#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "RASLib/init.h"
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/motor.h"
#include "RASLib/encoder.h"
 
void initMotors(void);
void initLineSensor(void);
void initUART(void);  
void Wait(long ms);
void SetDischargeTime(int ms);
unsigned char ReadLineSensor(void);
void UARTprintf(const char *pcString, ...);

void PID(){	  			 
	//This boolean is set to true if the background is black and line is white. False if the opposite
	tBoolean lineIsWhite=true;	
	//PID values   DO NOT CHANGE!!!
	float Kp=33;
	float Ki=.15;
	float Kd=15;
	int currentError=0,lastError=0;
	int integral=0;
	int derivative=0;
	//Line Sensor values
	unsigned char lineSensorByte;
 	int lineSensorArray[8];
	int i;
	//Motor Values
	signed char maxSpeed=127;

	//Random variables 
	int lineCount=0;
    int Pout;
	int Iout;
	int Dout;
	float MV;
	int x;
	float absMV;
	int m;
	signed char velocity;

	LockoutProtection();
	InitializeMCU();
	initMotors();	
	Wait(2000);
	SetMotorPowers(maxSpeed,maxSpeed);
	initLineSensor();
	initUART();
	SetDischargeTime(220);


	while(1){
		lastError=currentError;
		//Get line sensor values; 1 means black
		lineSensorByte = ReadLineSensor();
		if(lineIsWhite)lineSensorByte=~lineSensorByte;//Used to change between white and black lines
		for(i = 0; i < 8; i++) {
			lineSensorArray[i] = lineSensorByte & 0x01;
			lineSensorByte = lineSensorByte >> 1;
		}
		//Now it's time to compute the error... the algorithm is as follows:
		//Each bit has a value. Starting from the MSB and going right, the values are:
		//7, 5, 3, 1, -1, -3, -5, -7
		//These values are summed and averaged to get the error
		//NOTE: THIS IS FOR A BLACK LINE!!! FOR A WHITE LINE, INVERT THE VALUES
		currentError=0;
		lineCount=0;
		for(x=0;x<8;x++){
			currentError+=lineSensorArray[x]*(7-x*2);
			lineCount+=lineSensorArray[x];
		}  
		if(lineCount!=0)currentError/=lineCount;
		else{
			currentError=8;
			if(lastError<0)currentError*=-1;
		}
		UARTprintf("  %d  ",currentError);
		for(i = 0; i < 8; i++) 
			UARTprintf("%u",lineSensorArray[i]);
		UARTprintf("  %d              \r",currentError); 
		//Now that we have the error, we can update the derivative and integral
		derivative=currentError-lastError;
		integral+=currentError;
		//It is now time to compute Pout, Iout, and Dout, thus getting MV (the manipulated variable)
		//It should be noted that a positive derivative means the robot needs to turn left, and a
		//negative derivative means the robot needs to turn right. A positive integral means the
		//robot needs to skew left, and a negative integral value means the robot needs to skew right
		Pout=Kp*currentError;
		Iout=Ki*integral;
		Dout=Kd*derivative;
		MV=Pout+Iout+Dout;
		//If MV is positive, the robot needs to go left. If MV is negative, the robot needs to go right
		//But... we first need to come up with a linear relationship relating MV to the speed of the
		//reduced wheel (This means an mx+b relationship... more like a b-mx relationship)
		absMV=MV;
		if(MV<0)absMV*=-1;
		m=1;//This is the value that you change for sharper turning!!!
		velocity=(signed char)(maxSpeed-m*absMV);
		if(velocity<-128)velocity=-128;
		UARTprintf("%d     %f    ",velocity,absMV);
		if(MV>0){
			SetMotorPowers(maxSpeed,velocity);
		}
		else if(MV<0){
			SetMotorPowers(velocity,maxSpeed);
		}
		else{//MV equals 0; never going to happen, but might?
			SetMotorPowers(maxSpeed,maxSpeed);
		}
	}
}  
